//QUIRKSIM
//Copyright (c) 2008 Gregory Andrew Robinson (garobinson@ucdavis.edu)
//
//Permission is hereby granted, free of charge, to any person
//obtaining a copy of this software and associated documentation
//files (the "Software"), to deal in the Software without
//restriction, including without limitation the rights to use,
//copy, modify, merge, publish, distribute, sublicense, and/or sell
//copies of the Software, and to permit persons to whom the
//Software is furnished to do so, subject to the following
//conditions:
//
//The above copyright notice and this permission notice shall be
//included in all copies or substantial portions of the Software.
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
//
//EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
//OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
//NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
//HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
//WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
//FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
//OTHER DEALINGS IN THE SOFTWARE.

#ifndef _PARTICLE_CC
#define	_PARTICLE_CC

#include <algorithm>
#include <functional>
#include <math.h>
#include <glog/logging.h>
#include "particle.h"

//Implementing the undefined functions which Hep3Vector is _supposed_ to have.
//It is pretty absurd that I have to do this - is something wrong here?
//TODO(grobinson): Send these upstream if they really aren't in the codebase.
namespace CLHEP {

bool Hep3Vector::isNear(const Hep3Vector& v2, double epsilon) const {
  Hep3Vector diff = *this - v2;
  if (std::abs(diff.x()) > epsilon) {
    return false;
  }
  else if (std::abs(diff.y()) > epsilon) {
    return false;
  }
  else if (std::abs(diff.z()) > epsilon) {
    return false;
  }
  else {
    return true;
  }
}

Hep3Vector Hep3Vector::project(const Hep3Vector& v2) const {
  return v2.unit() * this->dot(v2);
}
}

namespace quirksim {
namespace particle {

template<typename T>
static bool deleteAll( T * item ) {
  delete item;
  return true;
}

MassiveParticle::MassiveParticle(const double m)
  : mass_(m),
    position_(Hep3Vector(0.0, 0.0, 0.0)),
    momentum_(Hep3Vector(0.0, 0.0, 0.0)) {
}

MassiveParticle::MassiveParticle(const double m, const Hep3Vector& x,
                                 const Hep3Vector& p)
  : mass_(m), position_(Hep3Vector(x)), momentum_(Hep3Vector(p)) {
}

MassiveParticle::~MassiveParticle() {
  std::remove_if(forces_.begin(), forces_.end(), deleteAll<ForceInterface>);
}

void MassiveParticle::Evolve(double dt) {
  Derivative a = EvaluateDerivs(Derivative(), 0);
  Derivative b = EvaluateDerivs(a, dt * 0.5);
  Derivative c = EvaluateDerivs(b, dt * 0.5);
  Derivative d = EvaluateDerivs(c, dt);
  LOG(INFO) << "a.dx.x()" << a.dx.x();
  LOG(INFO) << "b.dx.x()" << b.dx.x();
  LOG(INFO) << "c.dx.x()" << c.dx.x();
  LOG(INFO) << "d.dx.x()" << d.dx.x();

  Hep3Vector dxdt = 1.0 / 6.0 * (a.dx + 2.0 * (b.dx + c.dx) + d.dx);
  Hep3Vector dpdt = 1.0 / 6.0 * (a.dp + 2.0 * (b.dp + c.dp) + d.dp);

  LOG(INFO) << "dxdt = " << dxdt.x() << ", " << dxdt.y() << ", " << dxdt.z();
  LOG(INFO) << "dpdt = " << dpdt.x() << ", " << dpdt.y() << ", " << dpdt.z();

  set_position(position_ + dxdt * dt);
  set_momentum(momentum_ + dpdt * dt);
}

double MassiveParticle::GetEnergy(const Hep3Vector& p) const {
  // E^2 = p*p*c^2 + m^2*c^4
  return sqrt(p * p + pow(mass_, 2));
}

Hep3Vector MassiveParticle::GetNetForce(const Hep3Vector& x,
                                        const Hep3Vector& v) const {
  Hep3Vector netforce = Hep3Vector();
  std::vector<ForceInterface*>::const_iterator f_iter = forces_.begin();
  while(f_iter != forces_.end()) {
    netforce += (*f_iter)->GetForce(x, v);
    ++f_iter;
  }
  return netforce;
}

Hep3Vector MassiveParticle::GetVelocity(const Hep3Vector& p) const {
  return p / GetEnergy(p);
}

void MassiveParticle::RegisterForce(ForceInterface* f) {
  forces_.push_back(f);
}

QuirkString::QuirkString(const MassiveParticle& p1, const MassiveParticle& p2) :
  quirk1_(p1), quirk2_(p2) {}

StraightQuirkString::StraightQuirkString(const MassiveParticle& p1,
                                         const MassiveParticle& p2)
  : QuirkString(p1, p2) {}

QuirkPair::QuirkPair(MassiveParticle& ip1, MassiveParticle& ip2, const double s)
  : quirk1_(ip1),
    quirk2_(ip2),
    string_(new StraightQuirkString(ip1, ip2)),
    sigma_(s) {
  quirk1_.RegisterForce(new QuirkForce(*this, quirk1_));
  quirk2_.RegisterForce(new QuirkForce(*this, quirk2_));
}

QuirkPair::~QuirkPair() {
  delete string_;
}

QuirkForce::QuirkForce(const QuirkPair& iqp, const MassiveParticle& q)
  : pair_(iqp),
    quirk_(q) {}

Hep3Vector QuirkForce::GetForce(const Hep3Vector& x,
                                const Hep3Vector& v) const {
  const Hep3Vector dir = pair_.GetDirection(quirk_);
  const Hep3Vector v_perp = v.perpPart(dir);
  const Hep3Vector v_par = v.project(dir);

  LOG(INFO) << "v_perp=<" << v_perp.x() << ", " << v_perp.y() << ", "
             << v_perp.z() << ">";
  LOG(INFO) << "v_par=<" << v_par.x() << ", " << v_par.y() << ", "
             << v_par.z() << ">";

  Hep3Vector f = -1 * pair_.sigma() * (sqrt(1 - v_perp * v_perp) * v_par.unit()
                 + v_par.mag() / sqrt(1 - v_perp * v_perp) * v_perp);
  LOG(INFO) << "dp/dt <" << f.x() << ", " << f.y() << ", " << f.z() << ">";

  return -1 * pair_.sigma() * (sqrt(1 - v_perp * v_perp) * v_par.unit()
      + v_par.mag() / sqrt(1 - v_perp * v_perp) * v_perp);
}

} //namespace particle
} //namespace quirksim

#endif
